package computation;

import java.util.ArrayList;
import java.util.Random;

import org.lwjgl.util.vector.Matrix3f;
import org.lwjgl.util.vector.Vector3f;

import delaunay_triangulation.Delaunay_Triangulation;
import delaunay_triangulation.Point_dt;
import primatives.PointND;
import primatives.Vector;
import triangulation.DelaunayMapping;
import util.GeoTriangle;
import util.GeoVertex;

public class Geometry
{
	/**
	 * Compute the rotation matrix for a given angle and a dimension ('x', 'y', 'z')
	 * @param angle
	 * @param dimension
	 * @return rotation matrix
	 */
	public static double[][] computeRotationMatrix(double angle, char dimension)
	{
		double[][] rotMatrix = new double[3][3];
		double rad = Math.toRadians(angle);
		
		if (dimension == 'x')
		{
			rotMatrix[0][0] = 1;
			rotMatrix[0][1] = 0;
			rotMatrix[0][2] = 0;
			
			rotMatrix[1][0] = 0;
			rotMatrix[1][1] = Math.cos(rad);
			rotMatrix[1][2] = -Math.sin(rad);
			
			rotMatrix[2][0] = 0;
			rotMatrix[2][1] = Math.sin(rad);
			rotMatrix[2][2] = Math.cos(rad);
		}
		
		if (dimension == 'y')
		{
			rotMatrix[0][0] = Math.cos(rad);
			rotMatrix[0][1] = 0;
			rotMatrix[0][2] = Math.sin(rad);
			
			rotMatrix[1][0] = 0;
			rotMatrix[1][1] = 1;
			rotMatrix[1][2] = 0;
			
			rotMatrix[2][0] = -Math.sin(rad);
			rotMatrix[2][1] = 0;
			rotMatrix[2][2] = Math.cos(rad);
		}
		
		if (dimension == 'z')
		{
			rotMatrix[0][0] = Math.cos(rad);
			rotMatrix[0][1] = Math.sin(rad);
			rotMatrix[0][2] = 0;
			
			rotMatrix[1][0] = -Math.sin(rad);
			rotMatrix[1][1] = Math.cos(rad);
			rotMatrix[1][2] = 0;
			
			rotMatrix[2][0] = 0;
			rotMatrix[2][1] = 0;
			rotMatrix[2][2] = 1;
		}
		return rotMatrix;
	}
	
	
	public static int computeEps_DrawNumber(double scale, int dim, double eps)
	{
		double ratio = (dim/eps);
		int draw_number =  (int) (scale*ratio*Math.log(ratio)); // multiply by scale to allow more points by also achieve big O status
		return draw_number;
	}
	
	/*
	 * eps in [0,1]
	 * type determines the dimension of the eps-net
	 */
	public static ArrayList<PointND> computeEpsNet(DelaunayMapping mapping, String type, double scale, double eps)
	{
		ArrayList<PointND> epsNet = null;
		
		boolean bStrikeDip = type.equals("SnD");
		boolean bCurv_k1 = type.equals("k1");
		boolean bCurv_k2 = type.equals("k2");
		boolean bCurv_mean = type.equals("curv_mean");
		boolean bCurv_gaus = type.equals("curv_gaus");
		boolean bCurv_ansio = type.equals("curv_ansio");
		boolean bCurv_both = type.equals("curv_both");
		boolean bSnD_Curv = type.endsWith("SnD_Curv");
		boolean bNormal = type.endsWith("normal");

		int dim = 3; // assume x,y,z by default
		if (bStrikeDip) dim = 5;
		else if (bCurv_k1 || bCurv_k2 || bCurv_mean || bCurv_gaus || bCurv_ansio) dim = 4;
		else if (bCurv_both) dim = 5;
		else if (bSnD_Curv) dim = 6;
		else if (bNormal) dim = 6;
		
		
		int draw_number = computeEps_DrawNumber(scale, dim, eps);
		System.out.println("eps-Net draws: " + draw_number);
		
		if (draw_number < mapping.vertices.length)
		{
			epsNet = new ArrayList<PointND>(draw_number);
			Random rand = new Random(System.currentTimeMillis());
			for (int i = 0; i < draw_number; i++)
			{
				int index = rand.nextInt(mapping.vertices.length);
				GeoVertex gv = mapping.vertices[index];
				
				PointND pt = new PointND(dim);
				pt.set_coord(0, gv.x());
				pt.set_coord(1, gv.y());
				pt.set_coord(2, gv.z());
				
				if (bStrikeDip)
				{
					pt.set_coord(3, gv.avgStrike);
					pt.set_coord(4, gv.avgDip);
				}
				else if (bCurv_k1)
				{
				}
				else if (bCurv_k2)
				{
				}
				else if (bCurv_mean)
				{
					pt.set_coord(3, gv.curvatureMeanAveraged);
				}
				else if (bCurv_gaus)
				{
					pt.set_coord(3, gv.curvatureGaussian);
				}
				else if (bCurv_ansio)
				{
					pt.set_coord(3, gv.curvatureAnisotropicAveraged);
				}
				else if (bCurv_both)
				{
					pt.set_coord(3, gv.curvatureMeanAveraged);
					pt.set_coord(4, gv.curvatureGaussian);
				}
				else if (bSnD_Curv)
				{
					pt.set_coord(3, gv.avgStrike);
					pt.set_coord(4, gv.avgDip);
					pt.set_coord(5, gv.curvatureAnisotropic);
				}
				
				epsNet.add(pt);
			}
		}
		else
		{
			System.out.println("Not enough inital vertices!");
		}
		return epsNet;
	}
	
	
	public static float[] computeTriangleCentre(GeoVertex gv0, GeoVertex gv1, GeoVertex gv2)
	{
		float[] triangleCentre = new float[3];
		
		triangleCentre[0] = (float)((gv0.x() + gv1.x() + gv2.x())/3);
		triangleCentre[1] = (float)((gv0.y() + gv1.y() + gv2.y())/3);
		triangleCentre[2] = (float)((gv0.z() + gv1.z() + gv2.z())/3);
		
		return triangleCentre;
	}
	
	// likely the correct way to compute strike and dip.. useful to have computeStrikeNDipAngle to compare this to
	// adapted from: http://www.ceri.memphis.edu/people/olboyd/Software/bb.m
	public static double[] computeStrikeNDipAngle2(float[] n, double laserStrike, double laserDip)
	{
		double[] angles = new double[2];
		
		// *** in theory our normals SHOULD all point in the right direction => in the direction of the camera head ***
		// flip the sign of the normal vector if the normal points down
		if (n[2] < 0)
		{
			n[0] = -n[0];
			n[1] = -n[1];
			n[2] = -n[2];
		}
		
		double r2d = 180.0/Math.PI;
		double strike = 0.0;
		double strikeDeg = 0.0;
		
		double dip = 0.0;
		double dipDeg = 0.0;
		
		// if we have a laser head orientation let us recompute a new normal vector
		// and use Jason's way of computing the strike and dip
		if (laserStrike >= 0.0 && laserDip >= 0.0)
		{
			// TODO: use Matrix3f to define the rotation matrices
//			double[][] RotMatrix_Dip = new double[3][3]; // rotate about the X
//			RotMatrix_Dip[0][0] = 1;
//			RotMatrix_Dip[0][1] = 0;
//			RotMatrix_Dip[0][2] = 0;
//			
//			RotMatrix_Dip[1][0] = 0;
//			RotMatrix_Dip[1][1] = Math.cos(laserDip);
//			RotMatrix_Dip[1][2] = -Math.sin(laserDip);
//			
//			RotMatrix_Dip[2][0] = 0;
//			RotMatrix_Dip[2][1] = Math.sin(laserDip);
//			RotMatrix_Dip[2][2] = Math.cos(laserDip);
			
//			double[][] RotMatrix_Strike = new double[3][3]; // rotate about the Y
//			RotMatrix_Strike[0][0] = Math.cos(laserStrike);
//			RotMatrix_Strike[0][1] = 0;
//			RotMatrix_Strike[0][2] = Math.sin(laserStrike);
//			
//			RotMatrix_Strike[1][0] = 0;
//			RotMatrix_Strike[1][1] = 1;
//			RotMatrix_Strike[1][2] = 0;
//			
//			RotMatrix_Strike[2][0] = -Math.sin(laserStrike);
//			RotMatrix_Strike[2][1] = 0;
//			RotMatrix_Strike[2][2] = Math.cos(laserStrike);
			
			double[][] RotMatrix_Dip = computeRotationMatrix(laserDip, 'x');
			double[][] RotMatrix_Strike = computeRotationMatrix((-1)*laserStrike + 180, 'y');
			
			// apply the rotation matrix to the normal first in the X axis
	        double n1_x = n[0]*RotMatrix_Dip[0][0] + n[1]*RotMatrix_Dip[0][1] + n[2]*RotMatrix_Dip[0][2];
	        double n1_y = n[0]*RotMatrix_Dip[1][0] + n[1]*RotMatrix_Dip[1][1] + n[2]*RotMatrix_Dip[1][2];        
	        double n1_z = n[0]*RotMatrix_Dip[2][0] + n[1]*RotMatrix_Dip[2][1] + n[2]*RotMatrix_Dip[2][2];
	        
	        // then in the Y axis
	        double n2_x = n1_x*RotMatrix_Strike[0][0] + n1_y*RotMatrix_Strike[0][1] + n1_z*RotMatrix_Strike[0][2];
	        double n2_y = n1_x*RotMatrix_Strike[1][0] + n1_y*RotMatrix_Strike[1][1] + n1_z*RotMatrix_Strike[1][2];        
	        double n2_z = n1_x*RotMatrix_Strike[2][0] + n1_y*RotMatrix_Strike[2][1] + n1_z*RotMatrix_Strike[2][2];
	        
//	        System.out.println(RotMatrix_Dip[0][0] + " " + RotMatrix_Dip[0][1] + " " + RotMatrix_Dip[0][2]);
//	        System.out.println(RotMatrix_Dip[1][0] + " " + RotMatrix_Dip[1][1] + " " + RotMatrix_Dip[1][2]);
//	        System.out.println(RotMatrix_Dip[2][0] + " " + RotMatrix_Dip[2][1] + " " + RotMatrix_Dip[2][2]);
//	        System.out.println();
//	        System.out.println(RotMatrix_Strike[0][0] + " " + RotMatrix_Strike[0][1] + " " + RotMatrix_Strike[0][2]);
//	        System.out.println(RotMatrix_Strike[1][0] + " " + RotMatrix_Strike[1][1] + " " + RotMatrix_Strike[1][2]);
//	        System.out.println(RotMatrix_Strike[2][0] + " " + RotMatrix_Strike[2][1] + " " + RotMatrix_Strike[2][2]);
	        
	        // quadrant variables
	        int q_s = 0;
	        int q_d = 0;
	        
	        // strike quadrant
	        if (n2_x >= 0 && n2_z <= 0) q_s = 1; // all
	        else if (n2_x <=0 && n2_z <= 0) q_s = 2; // sin
	        else if (n2_x <=0 && n2_z >= 0) q_s = 3; // tan
	        else if (n2_x >= 0 && n2_z >= 0) q_s = 4; // cos

	        // dip quadrant
	        if (n2_y >= 0.0) q_d = 1;
	        else if (n2_y < 0.0) q_d = -1;
	        
	        // compute the dip
	        dip = Math.atan(n2_y/Math.sqrt(n2_x*n2_x + n2_z*n2_z));
	        dipDeg = dip * r2d;
	        
	        // dip corrections
	        if (dip >= 0.0 && dip <= Math.PI/2)  q_d = 1; // Initial condition, down slope
	        else if (dip > Math.PI/2)
	        {
	            dip = Math.PI - dip;
	            dipDeg = 180.0 - dipDeg;
	            q_d = -1;      	
	        }
	        else if (dip < 0.0)
	        {
	            dip = (-1) * dip;
	            dipDeg = (-1) * dipDeg;
	            q_d = -1;	        	
	        }
	        
	        // strike correction based on dip > 90 or dip < 0
	        if (q_d == -1)
	        {
	            if (q_s == 1) q_s = 3;
	            else if (q_s == 2) q_s = 4;
	            else if (q_s == 3) q_s = 1;
	            else if (q_s == 4) q_s = 2;
	            q_d = 1;
	        }
	        
	        // compute the strike
	        strike = Math.abs(Math.atan(n2_z/n2_x));
	        
	        if (q_s == 4) strike = (3.0/2.0) * Math.PI + strike;
	        else if (q_s == 3) strike = (1.0/2.0) * Math.PI - strike;
	        else if (q_s == 2) strike = (1.0/2.0) * Math.PI + strike;
	        else if (q_s == 1) strike = (3.0/2.0) * Math.PI - strike;
	        if (strike >= (2.0) * Math.PI) strike = (2.0) * Math.PI - strike;
	        
			dip = 0.5 * Math.PI - dip;
			dipDeg = 90.0 - dipDeg;
			
			strikeDeg = strike * r2d;
			dipDeg = dip * r2d;
		}
		else // use the simple atan2 way
		{
			strike = Math.atan2(n[0], n[1]);
			strikeDeg = strike * r2d;
			
			dip = Math.atan2(Math.sqrt(n[1]*n[1] + n[0]*n[0]), n[2]);
			dipDeg = dip * r2d;
			
			// go round and round until we are in range
			strikeDeg = strikeDeg - 90.0;
			while (strikeDeg >= 360) strikeDeg = strikeDeg - 360.0;
			while (strikeDeg < 0) strikeDeg = strikeDeg + 360.0;
		}
		
		angles[0] = strikeDeg;
		angles[1] = dipDeg;
		
//		System.out.println("Strike: " + strikeDeg + "\t\tDip:" + dipDeg);
		return angles;
	}
	
	
	public static float[] computeStrikeVector(float[] n)
	{
		float[] strikeVec = new float[3];
		double[] unit = {0.0, 0.0, 1.0};
		strikeVec[0] = (float)((unit[1] * n[2]) - (unit[2] * n[1]));
		strikeVec[1] = (float)((unit[2] * n[0]) - (unit[0] * n[2]));
		strikeVec[2] = (float)((unit[0] * n[1]) - (unit[1] * n[0]));
		return strikeVec;
	}
	
	// compute the normal of any given 3 points
	public static float[] computeSurfaceNormal(Point_dt v0, Point_dt v1, Point_dt v2)
	{
		float[] normal = {0.0f, 0.0f, 0.0f};
		Vector p0 = new Vector(new double[] {v0.x(), v0.y(), v0.z()});
		Vector p1 = new Vector(new double[] {v1.x(), v1.y(), v1.z()});
		Vector p2 = new Vector(new double[] {v2.x(), v2.y(), v2.z()});
		p0.enableImmutable(); p1.enableImmutable(); p2.enableImmutable();
		
		Vector U = p1.minus(p0);
		Vector V = p2.minus(p0);
		normal[0] = (float)((U.get(1) * V.get(2)) - (U.get(2) * V.get(1)));
		normal[1] = (float)((U.get(2) * V.get(0)) - (U.get(0) * V.get(2)));
		normal[2] = (float)((U.get(0) * V.get(1)) - (U.get(1) * V.get(0)));
		
		return normal;
	}
	
	// TODO: convert all the places that use computeSurfaceNormal(Point_dt,Point_dt,Point_dt) to use this function
	public static float[] computeSurfaceNormal(GeoVertex v0, GeoVertex v1, GeoVertex v2)
	{
		float[] normal = {0.0f, 0.0f, 0.0f};
		Vector p0 = new Vector(new double[] {v0.x(), v0.y(), v0.z()});
		Vector p1 = new Vector(new double[] {v1.x(), v1.y(), v1.z()});
		Vector p2 = new Vector(new double[] {v2.x(), v2.y(), v2.z()});
		p0.enableImmutable(); p1.enableImmutable(); p2.enableImmutable();
		
		Vector U = p1.minus(p0);
		Vector V = p2.minus(p0);
		normal[0] = (float)((U.get(1) * V.get(2)) - (U.get(2) * V.get(1)));
		normal[1] = (float)((U.get(2) * V.get(0)) - (U.get(0) * V.get(2)));
		normal[2] = (float)((U.get(0) * V.get(1)) - (U.get(1) * V.get(0)));
		
		return normal;
	}
	
	// compute the normal of a vertex by averaging the normals of the incident triangles
	public static float[] computeVertexNormal(GeoVertex v, DelaunayMapping mapping)
	{
		float[] normal = {0.0f, 0.0f, 0.0f};
		for (Integer tk: v.trianglekeys)
		{
			GeoTriangle gt = mapping.triangles[tk];
			GeoVertex gv0 = mapping.vertices[gt.vertexkeys[0]];
			GeoVertex gv1 = mapping.vertices[gt.vertexkeys[1]];
			GeoVertex gv2 = mapping.vertices[gt.vertexkeys[2]];
			
			float[] n = computeSurfaceNormal(gv0, gv1, gv2);
			normal[0] += n[0];
			normal[1] += n[1];
			normal[2] += n[2];
		}
		normal[0] /= v.trianglekeys.size();
		normal[1] /= v.trianglekeys.size();
		normal[2] /= v.trianglekeys.size();
		
		return normal;
	}
	
	public static float vectorMagnitude(float[] vec) { return (float) Math.sqrt((vec[0]*vec[0]) + (vec[1]*vec[1]) + (vec[2]*vec[2])); }
	public static double vectorMagnitude(double[] vec) { return Math.sqrt((vec[0]*vec[0]) + (vec[1]*vec[1]) + (vec[2]*vec[2])); }
	
	public static float[] vectorNormalize(float[] vec)
	{
		float[] normalized = new float[3];
		float mag = vectorMagnitude(vec);
		normalized[0] = vec[0]/mag;
		normalized[1] = vec[1]/mag;
		normalized[2] = vec[2]/mag;
		return normalized;
	}
	
    public static double[] vectorMinus(GeoVertex A, GeoVertex B)
    {
    	double[] ret = new double[3];
    	ret[0] = A.x() - B.x();
    	ret[1] = A.y() - B.y();
    	ret[2] = A.z() - B.z();
        return ret;
    }
    
    public static double[] vectorMinus(double[] A, double[] B)
    {
    	double[] ret = new double[3];
    	ret[0] = A[0] - B[0];
    	ret[1] = A[1] - B[1];
    	ret[2] = A[2] - B[2];
        return ret;
    }
    
    public static float[] vectorMinus(float[] A, float[] B)
    {
    	float[] ret = new float[3];
    	ret[0] = A[0] - B[0];
    	ret[1] = A[1] - B[1];
    	ret[2] = A[2] - B[2];
        return ret;
    }
    
    public static float[] vectorPlus(float[] A, float[] B)
    {
    	float[] ret = new float[3];
    	ret[0] = A[0] + B[0];
    	ret[1] = A[1] + B[1];
    	ret[2] = A[2] + B[2];
        return ret;    	
    }
    
    public static double[] vectorPlus(double[] A, double[] B)
    {
    	double[] ret = new double[3];
    	ret[0] = A[0] + B[0];
    	ret[1] = A[1] + B[1];
    	ret[2] = A[2] + B[2];
        return ret;    	
    }
	
	public static double[] crossProduct(GeoVertex a, GeoVertex b)
	{
		double[] ret = new double[3];
		ret[0] = a.y()*b.z() - a.z()*b.y();
		ret[1] = a.z()*b.x() - a.x()*b.z();
		ret[2] = a.x()*b.y() - a.y()*b.x();
		return ret;
	}
	
	public static double[] crossProduct(double[] a, double[] b)
	{
		double[] ret = new double[3];
		ret[0] = a[1]*b[2] - a[2]*b[1];
		ret[1] = a[2]*b[0] - a[0]*b[2];
		ret[2] = a[0]*b[1] - a[1]*b[0];
		return ret;
	}
	
	public static double[] crossProduct(float[] a, float[] b)
	{
		double[] ret = new double[3];
		ret[0] = a[1]*b[2] - a[2]*b[1];
		ret[1] = a[2]*b[0] - a[0]*b[2];
		ret[2] = a[0]*b[1] - a[1]*b[0];
		return ret;
	}
	
	public static double triangleArea(GeoVertex v0, GeoVertex v1, GeoVertex v2)
	{
		// Area = 1/2 | (v2 - v0) X (v2 - v1) | where X denotes the cross product
		double[] diff1 = vectorMinus(v2, v0);
		double[] diff2 = vectorMinus(v2, v1);
		double[] cross = crossProduct(diff1, diff2);
		return vectorMagnitude(cross)/2;
	}
	
	public static double triangleArea(float[] A, float[] B, float[] C)
	{
		float[] diff1 = vectorMinus(C, A);
		float[] diff2 = vectorMinus(C, B);
		double[] cross = crossProduct(diff1, diff2);
		return vectorMagnitude(cross)/2;
	}
	
	// 2PI - sum of angles between all the edges incident on v
	public static double angleOfDefect(GeoVertex v, DelaunayMapping mapping)
	{
		double defect = 0.0;
		
		// order matters thus cannot use neighbourkeys as with a hash you will the keys in random order thus
		// making angle calculations inaccurate
		for (Integer tk: v.trianglekeys) 
		{
			GeoTriangle gt = mapping.triangles[tk];
			
			GeoVertex gv0 = mapping.vertices[gt.vertexkeys[0]];
			GeoVertex gv1 = mapping.vertices[gt.vertexkeys[1]];
			GeoVertex gv2 = mapping.vertices[gt.vertexkeys[2]];
			
			GeoVertex a0 = null;
			GeoVertex a1 = null;
			
			if (gv0.equals(v))
			{
				a0 = gv1;
				a1 = gv2;
			}
			
			if (gv1.equals(v))
			{
				a0 = gv0;
				a1 = gv2;
			}
			
			if (gv2.equals(v))
			{
				a0 = gv0;
				a1 = gv1;
			}
			
			// compute the direction vectors so that we can use the dot produce to find the angle between them
			double[] v0 = new double[3];
			double[] v1 = new double[3];
			double dotproduct = 0.0;
			
			v0[0] = v.x() - a0.x();
			v0[1] = v.y() - a0.y();
			v0[2] = v.z() - a0.z();
			
			v1[0] = v.x() - a1.x();
			v1[1] = v.y() - a1.y();
			v1[2] = v.z() - a1.z();

			for (int i = 0; i < 3; i++) { dotproduct += (v0[i] * v1[i]); }
			double angle = Math.acos(dotproduct / (vectorMagnitude(v0) * vectorMagnitude(v1)));
			defect += Math.toDegrees(angle);
		}
		return Math.toDegrees(Math.PI*2) - defect;
	}
	
	// returns the parameter needed to find the intersection point between s and e wrt to a circle centered at p with radius r
	public static double SphereLineIntersection(GeoVertex p, GeoVertex s, GeoVertex e, double r)
	{
		double x1 = s.x();
		double y1 = s.y();
		double z1 = s.z();
		
		double x2 = e.x();
		double y2 = e.y();
		double z2 = e.z();
		
		double x3 = p.x();
		double y3 = p.y();
		double z3 = p.z();
		
		
		// a = (x2-x1)^2 + (y2-y1)^2 + (z2-z1)^2
		double a = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) + (z2-z1)*(z2-z1);
		
		// b = 2[ (x2 - x1) (x1 - x3) + (y2 - y1) (y1 - y3) + (z2 - z1) (z1 - z3) ]
		double b = 2.0*( (x2 - x1)*(x1 - x3) + (y2 - y1)*(y1 - y3) + (z2 - z1)*(z1 - z3) ) ;
		
		// c = x32 + y32 + z32 + x12 + y12 + z12 - 2[x3 x1 + y3 y1 + z3 z1] - r2
		double c = x3*x3 + y3*y3 + z3*z3 + x1*x1 + y1*y1 + z1*z1 - 2.0*(x3*x1 + y3*y1 + z3*z1) - r*r;
		
		double det = b*b - 4*a*c;
	
		
//		if (det < 0) return -1; // no intersection
////		else if (det == 0.0 || det < 0.00001)
//		else // unlikely that there will be two intersection points??
//		{
//			double u = (-b/2*a);
//			if (u < 0 || u > 1) return -1;
//			
//			
//			return u; // intersection at one point: s + u(e-s) where u = (-b/2*a) 
//		}
		
//		double u = ((x3 - x1)*(x2 - x1) + (y3 - y1)*(y2 - y1) + (z3 - z1)*(z2 - z1))/((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1) + (z2 - z1)*(z2 - z1));
		
		double u1 = (-b + Math.sqrt(det))/(2*a);
		double u2 = (-b - Math.sqrt(det))/(2*a);
		
		return u1;
	}
}
